#include "ros/ros.h"
#include "sensor_msgs/JointState.h"
#include "mbot_linux_serial.h" 

unsigned char testSend3=0x07;
float pos[5] = {0};
int joint[5] = {0}; 

void Callback(const sensor_msgs::JointStateConstPtr& msg)
{
    int len = msg->position.size();
    for (int j = 0; j < len && j < 5; j++) {
        pos[j] = msg->position[j];
    }
    if (len >= 5) {
        ROS_INFO("pos,%f,%f,%f,%f,%f", pos[0], pos[1], pos[2], pos[3], pos[4]);
        for (int i = 0; i < 5; i++) {
            joint[i] = static_cast<int>(pos[i] * 10000);
        }
        writeSpeed(joint, testSend3); // 
        ROS_INFO("Data was transmitted.\n");
    }
}

int main(int agrc,char **argv)
{
    ros::init(agrc,argv,"public_node");
    ros::NodeHandle nh;

    ros::Rate loop_rate(100);//设置循环频率为100Hz
    
    Serial_Init();//串口初始化

    while(ros::ok())
    {
        ros::Subscriber sub = nh.subscribe("/joint_states", 10000, Callback); 
        ros::spin();
        loop_rate.sleep();
    }
    return 0;
}
